package com.iaroc.irobot;


import java.io.IOException;

import com.sun.spot.peripheral.TimeoutException;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IIOPin;
import com.sun.spot.util.BootloaderListener;
import com.sun.spot.util.Utils;
import com.sun.squawk.util.UnexpectedException;

/**
 * Represents the top level class used to communicate with the Create. We did
 * have an interface definint this interface at one point, but it got removed to
 * "simplify" the code, reduce number of classes.
 * 
 * The communication channel is seperated out in order for us to be able to
 * support different modes of connecting to the Create.
 * 
 */
public class IRobotCreateSerialConnection {
    protected EDemoBoard demoBoard;
    protected IIOPin baudRateSelectPin;
    protected boolean debug;
    protected int[] recordedBytes;
    protected int recordedBytesCount;
    protected boolean isRecording;
    protected byte[] uartBuffer;

    public IRobotCreateSerialConnection(EDemoBoard demoBoard, IIOPin baudRateSelectPin, boolean debug) {
        this.debug = debug;
        this.demoBoard = demoBoard;
        this.baudRateSelectPin = baudRateSelectPin;
        new BootloaderListener().start();
        this.recordedBytes = new int[IRobotCreateConstants.MAX_COMMAND_SIZE];
        this.recordedBytesCount = 0;
        isRecording = false;
        uartBuffer = new byte[IRobotCreateConstants.MAX_COMMAND_SIZE];
        try {
            connectToCreate();
        } catch (UnexpectedException e) {
            if (debug) {
                System.out.println("Try connecting one more time in case user forgot to turn on the Create");
            }
            Utils.sleep(2500);
            connectToCreate();
        }
    }

    protected void connectToCreate() {
        if (baudRateSelectPin == null) {
            if (debug) {
                System.out.println("Changing baud rate via command");
            }
            demoBoard.initUART(EDemoBoard.SERIAL_SPEED_57600, false);
            demoBoard.writeUART((byte) IRobotCreateConstants.COMMAND_BAUD);
            demoBoard.writeUART((byte) IRobotCreateConstants.BAUD_RATE_19200);
            Utils.sleep(100);
            demoBoard.initUART(EDemoBoard.SERIAL_SPEED_19200, false);
            Utils.sleep(100);
        } else {
            demoBoard.initUART(EDemoBoard.SERIAL_SPEED_19200, false);
            if (debug) {
                System.out.println("Triggering baud rate select pin");
            }
            baudRateSelectPin.setAsOutput(true);
            for (int i = 0; i < 4; i++) {
                baudRateSelectPin.setLow();
                Utils.sleep(50);
                baudRateSelectPin.setHigh();
                Utils.sleep(50);
            }
        }
        baudRateSelectPin.setLow();
        int readCount = 0;
        int errorCount = 0;
        while (true) {
            try {
                int read = demoBoard.readUART(100);
                if (debug) {
                    System.out.print('[');
                    System.out.print(readCount);
                    System.out.print("]=");
                    System.out.print(read);
                    System.out.print(',');
                    System.out.println((char) read);
                }
                readCount++;
            } catch (TimeoutException e) {
                break;
            } catch (IOException e) {
                errorCount++;
            }
        }
        if (debug) {
            System.out.print("connectToCreate: errorCount=");
            System.out.println(errorCount);
        }
        final int numberOfStartsToSend = IRobotCreateConstants.MAX_COMMAND_SIZE;
//        final int numberOfStartsToSend = 1;
        for (int i=0; i < numberOfStartsToSend; i++) {
            uartBuffer[i] = (byte) IRobotCreateConstants.COMMAND_START;
        }
        sendBytes(uartBuffer, 0, numberOfStartsToSend);
        if (debug) {
            System.out.println("Waiting for create to get into passive mode");
        }
        while (true) {
            if (debug) {
                System.out.println("sensors(SENSORS_OI_MODE)");
            }
            uartBuffer[0] = (byte) IRobotCreateConstants.COMMAND_SENSORS;
            uartBuffer[1] = (byte) IRobotCreateConstants.SENSORS_OI_MODE;
            sendBytes(uartBuffer, 0, 2);
            int mode = readUnsignedByte();
            if (mode == IRobotCreateConstants.OI_MODE_PASSIVE) {
                break;
            }
        }
    }
    
    public int[] getRecordedBytes() {
        return recordedBytes;
    }
    
    public int getRecordedBytesCount() {
        return recordedBytesCount;
    }
    
    public boolean isRecording() {
        return isRecording;
    }

    public int readSignedByte() {
        if (debug) {
            System.out.println("read signed byte: ");
        }
        try {
            int result = demoBoard.readUART();
            if (debug) {
                System.out.print("   ");
                System.out.print(result);
                System.out.print(':');
                System.out.println((char) result);
            }
            return result;
        } catch (IOException e) {
            throw new UnexpectedException(e);
        }
    }

    public int readUnsignedByte() {
        if (debug) {
            System.out.println("read unsigned byte: ");
        }
        try {
            int result = demoBoard.readUART() & 0xFF;
            if (debug) {
                System.out.print("   ");
                System.out.print(result);
                System.out.print(':');
                System.out.println((char) result);
            }
            return result;
        } catch (IOException e) {
            throw new UnexpectedException(e);
        }
    }

    public int readSignedWord() {
        if (debug) {
            System.out.println("read signed word: ");
        }
        try {
            int high = demoBoard.readUART();
            int low = demoBoard.readUART();
            // Java is already twos complement, so no need for any translation
            int signed = (high << 8) | (low & 0xFF);
            if (debug) {
                System.out.print("   ");
                System.out.print(high & 0xFF);
                System.out.print(':');
                System.out.print((char) high);
                System.out.print(',');
                System.out.print(low & 0xFF);
                System.out.print(':');
                System.out.print((char) low);
                System.out.print("->");
                System.out.println(signed);
            }
            return signed;
        } catch (IOException e) {
            throw new UnexpectedException(e);
        }
    }
    
    public int readUnsignedBytes(int[] buffer, int start, int length) {
        if (debug) {
            System.out.print("read unsigned bytes: ");
            System.out.println(length);
        }
        if (length > uartBuffer.length) {
            uartBuffer = new byte[length];
        }
        try {
            int readCount = demoBoard.readUART(uartBuffer, 0, length);
            for (int i=0; i < length; i++) {
                buffer[start + i] = uartBuffer[i] & 0xFF;
            }
            if (debug) {
                for (int i=0; i < length; i++) {
                    System.out.print('[');
                    System.out.print(i);
                    System.out.print("]=");
                    System.out.println(buffer[start + i]);
                }
            }
            return readCount;
        } catch (IOException e) {
            throw new UnexpectedException(e);
        }
    }

    public int readUnsignedWord() {
        if (debug) {
            System.out.println("read unsigned word: ");
        }
        try {
            int high = demoBoard.readUART();
            int low = demoBoard.readUART();
            int unsigned = ((high & 0xFF)<< 8) | (low & 0xFF);
            if (debug) {
                System.out.print("   ");
                System.out.print(high & 0xFF);
                System.out.print(':');
                System.out.print((char) high);
                System.out.print(',');
                System.out.print(low & 0xFF);
                System.out.print(':');
                System.out.print((char) low);
                System.out.print("->");
                System.out.println(unsigned);
            }
            return unsigned;
        } catch (IOException e) {
            throw new UnexpectedException(e);
        }
    }
    
    public void sendByte(int b) {
        if (isRecording) {
            recordedBytes[recordedBytesCount++] = b;
        } else {
            if (debug) {
                System.out.print("sending: ");
                System.out.print(b);
                System.out.print(':');
                System.out.print(b & 0xFF);
                System.out.print(':');
                System.out.println((char) b);
            }
            demoBoard.writeUART((byte) b);
            Utils.sleep(1);
        }
    }
    
    public void sendBytes(byte[] bytes, int start, int length) {
        if (isRecording) {
            for (; length > 0; start++, length--) {
                recordedBytes[recordedBytesCount++] = bytes[start];
            }
        } else {
            if (debug) {
                System.out.print("sendBytes byte[] length: ");
                System.out.println(length);
                for (int i=0; i < length; i++) {
                    System.out.print('[');
                    System.out.print(i);
                    System.out.print("]: ");
                    System.out.print(bytes[start + i]);
                    System.out.print(':');
                    System.out.print(bytes[start + i] & 0xFF);
                    System.out.print(':');
                    System.out.println((char) bytes[start + i]);
                }
            }
            demoBoard.writeUART(bytes, start, length);
        }
    }
    
    public void sendBytes(int[] bytes, int start, int length) {
        if (isRecording) {
            for (; length > 0; start++, length--) {
                recordedBytes[recordedBytesCount++] = bytes[start];
            }
        } else {
            if (debug) {
                System.out.print("sendBytes int[] length: ");
                System.out.println(length);
            }
            if (length > uartBuffer.length) {
                uartBuffer = new byte[length];
            }
            for (int i=0; i < length; i++) {
                uartBuffer[i] = (byte) bytes[start + i];
                if (debug) {
                    System.out.print('[');
                    System.out.print(i);
                    System.out.print("]: ");
                    System.out.print(bytes[start + i]);
                    System.out.print(':');
                    System.out.println((char) bytes[start + i]);
                }
            }
            demoBoard.writeUART(uartBuffer, 0, length);
        }
    }
    

    public void sendSignedWord(int value) {
        // Java bit representation is already two's complement
        uartBuffer[0] = (byte) (value >> 8);
        uartBuffer[1] = (byte) (value & 0xFF);
        demoBoard.writeUART(uartBuffer, 0, 2);
        if (debug) {
            System.out.print("send signed word");
            System.out.println(value);
        }
    }
    
    public void sendUnsignedWord(int value) {
        uartBuffer[0] = (byte) (value >> 8);
        uartBuffer[1] = (byte) (value & 0xFF);
        demoBoard.writeUART(uartBuffer, 0, 2);
        if (debug) {
            System.out.print("send unsigned word");
            System.out.print(value);
        }
    }
    
    public void afterCommandPause() {
        if(isRecording) {
            return;
        }
        if (debug) {
            System.out.println("afterCommandPause");
        }
        Utils.sleep(IRobotCreateConstants.AFTER_COMMAND_PAUSE_TIME);
    }

    public void setDebug(boolean debug) {
        this.debug = debug;
    }

    public void startRecording() {
        isRecording = true;
    }
    
    public int stopRecording() {
        isRecording = false;
        int count = recordedBytesCount;
        recordedBytesCount = 0;
        return count;
    }
    
}
